#include <ModuleHandler.h>
#include <TBranch.h>
#include <TFile.h>
#include <TTree.h>
#include <sys/socket.h>
#include <sys/types.h>
#include <sys/time.h>
#include <netdb.h>
#include <string.h>
#include <iostream>
#include <iomanip>
#include <errno.h>
#include <sstream>
#include <fcntl.h>
#include <termios.h>
#include <math.h>
#include <sys/stat.h>
using namespace std;

string DATA_PATH;

enum axis {
	X = 1, Y = 2, Z = 4
};

struct identifiers {
	ModuleHandler* handler;
	int xStatMC;
	int yStatMC;
	int zStatMC;
	int motorStatus;
	int zeroCore;
	int xZeroCore;
	int yZeroCore;
	int zZeroCore;
	int readCore;
	int xReadCore;
	int yReadCore;
	int zReadCore;
	int marioControl;
	int mm10vX;
	int mm10vY;
	int mm10vZ;
	int mmRead;
	int MCCContElem;
	int MCCIRangeCont;
	int MCCJRangeCont;
	int MCCKRangeCont;
	int MCCStep;
	int MCCFile;
	int MCTContElem;
	int MCTIRangeCont;
	int MCTKRangeCont;
	int MCTStep;
	int MCTFile;
	int MOTContElem;
	int MOTTimeCont;
	int MOTFileCont;
	int workerRunElem;
	int workerRunStatus;
	int workerRunTimeStatus;
	int workerRunNameStatus;
	int advance;
	int xad;
	int yad;
	int zad;
};

identifiers id;
pthread_rwlock_t runMainLock;
bool runMainBool;
pthread_t worker;
pthread_rwlock_t runWorkerLock;
bool runWorkerBool;
bool joinWorker;

bool runMain() {
	pthread_rwlock_rdlock(&runMainLock);
	bool run = runMainBool;
	pthread_rwlock_unlock(&runMainLock);
	return run;
}

void setMain(bool val) {
	pthread_rwlock_wrlock(&runMainLock);
	runMainBool = val;
	pthread_rwlock_unlock(&runMainLock);
}

bool runWorker() {
	pthread_rwlock_rdlock(&runWorkerLock);
	bool run = runWorkerBool;
	pthread_rwlock_unlock(&runWorkerLock);
	return run;
}

void setWorker(bool val) {
	pthread_rwlock_wrlock(&runWorkerLock);
	runWorkerBool = val;
	pthread_rwlock_unlock(&runWorkerLock);
}

class Cooldown {
public:
	Cooldown() {
		energy[0] = 0;
		energy[1] = 0;
		energy[2] = 0;
		timeval t;
		gettimeofday(&t, NULL);
		timestamp = t.tv_sec + ((double) t.tv_usec) / 1000000;
		coolRate = .7;
		limit = 10;
	}
	~Cooldown() {

	}

	void wait(unsigned int axes) {
		cool();
		int waitTime = 0;

		if ((axes & X) == X) {
			if (energy[0] > limit) {
				waitTime = round((energy[0] - limit) / coolRate * 1000000);
			}
		}
		if ((axes & Y) == Y) {
			int waitTimeTemp = 0;
			if (energy[1] > limit) {
				waitTimeTemp = round((energy[1] - limit) / coolRate * 1000000);
			}
			if (waitTimeTemp > waitTime)
				waitTime = waitTimeTemp;
		}
		if ((axes & Z) == Z) {
			int waitTimeTemp = 0;
			if (energy[2] > limit) {
				waitTimeTemp = round((energy[2] - limit) / coolRate * 1000000);
			}
			if (waitTimeTemp > waitTime)
				waitTime = waitTimeTemp;
		}

		//cout << "waiting " << waitTime << " microseconds" << endl;
		usleep(waitTime);
	}

	void registerTime(unsigned int axes, double time) {
		if ((axes & X) == X) {
			energy[0] += time;
		}
		if ((axes & Y) == Y) {
			energy[1] += time;
		}
		if ((axes & Z) == Z) {
			energy[2] += time;
		}
	}

private:
	void cool() {
		timeval t;
		gettimeofday(&t, NULL);
		double newTimestamp = t.tv_sec + ((double) t.tv_usec) / 1000000;
		for (int i = 0; i < 3; i++) {
			energy[i] -= (newTimestamp - timestamp) * coolRate;
			if (energy[i] < 0)
				energy[i] = 0;
		}
		timestamp = newTimestamp;
	}

	double energy[3];
	double timestamp;
	double coolRate;
	double limit;
};

class Voltron {
public:
	Voltron(string xip, string yip, string zip) {
		connectToMM(xsock, xip, xcon);
		connectToMM(ysock, yip, ycon);
		connectToMM(zsock, zip, zcon);
	}

	~Voltron() {
		if (xcon)
			close(xsock);
		if (ycon)
			close(ysock);
		if (zcon)
			close(zsock);
	}

	void connectToMM(int& sock, string ip, bool& con) {
		addrinfo hints;
		addrinfo* res;
		memset(&hints, 0, sizeof hints);
		hints.ai_family = AF_UNSPEC;
		hints.ai_socktype = SOCK_STREAM;
		getaddrinfo(ip.c_str(), "5024", &hints, &res);
		sock = socket(res->ai_family, res->ai_socktype, res->ai_protocol);
		if (connect(sock, res->ai_addr, res->ai_addrlen) > -1)
			con = true;
		else
			con = false;
		freeaddrinfo(res);
		getResponse(sock, con);
	}

	string getResponse(int& sock, bool& con) {
		int maxLength = 0xfff;
		char* buffer = new char[maxLength];
		buffer[0] = '\0';
		int recvLength = -1;
		if (con)
			recvLength = recv(sock, buffer, maxLength, 0);
		if (recvLength == -1) {
			con = false;
			delete[] buffer;
			return "";
		}
		buffer[recvLength] = '\0';
		string response = buffer;
		size_t endPos = 0;
		while (true) {
			endPos = response.find("3441xA> ");
			if (endPos != string::npos)
				break;

			recvLength = recv(sock, buffer, maxLength, 0);
			if (recvLength == -1) {
				con = false;
				delete[] buffer;
				return "";
			}
			if (recvLength < 1)
				break;
			buffer[recvLength] = '\0';
			response += buffer;
		}
		delete[] buffer;
		return response.substr(0, endPos);
	}

	string read10V(int axis) {
		string retVal = "";
		string command = "MEAS:VOLT:DC? 10v\r\n";
		switch (axis) {
		case X:
			if (xcon)
				if (send(xsock, command.c_str(), command.length(), 0) == -1) {
					xcon = false;
					return "";
				}
			retVal = getResponse(xsock, xcon);
			break;
		case Y:
			if (ycon) {
				if (send(ysock, command.c_str(), command.length(), 0) == -1) {
					ycon = false;
					return "";
				}
			}
			retVal = getResponse(ysock, ycon);
			break;
		case Z:
			if (zcon) {
				if (send(zsock, command.c_str(), command.length(), 0) == -1) {
					zcon = false;
					return "";
				}
			}
			retVal = getResponse(zsock, zcon);
			break;
		default:
			;
		}
		size_t commandPos = retVal.find(command);
		if (commandPos != string::npos)
			retVal = retVal.substr(commandPos + command.length(), retVal.length() - commandPos - command.length() - 2);
		else
			retVal = "";
		return retVal;
	}

	bool connected() {
		if (xcon && ycon && zcon)
			return true;
		return false;
	}

private:
	int xsock;
	int ysock;
	int zsock;
	bool xcon;
	bool ycon;
	bool zcon;
};

class MotorControl {
public:
	MotorControl() {
		cd = new Cooldown();
		watchRunning = false;

		//Define default values for Acceleration and speed and initialize position monitors for Mario Song.
		accel = "A1M1\rA2M1\rA3M1\r";
		speed = "S1M6000\rS2M6000\rS3M6000\r";
		twoSongPos = 0;
		threeSongPos = 0;

		//Define axes
		xaxis = 3;
		yaxis = 2;
		zaxis = 1;

		//Open the connection to the serial port, you may have to change device
		fd = open(/*"/dev/ttyUSB0"*/"/dev/ttyS0", O_RDWR | O_NOCTTY);
		if (fd == -1) {
			;//cout << "Cannot connect to serial port" << endl;
		} else {
			//Set options for communication with the serial port.
			fcntl(fd, F_SETFL, 0);
			tcgetattr(fd, &options);
			cfsetspeed(&options, B9600); //Set baud rate to 9600
			cfmakeraw(&options);
			options.c_cflag &= ~CSTOPB; //use one stop bit
			options.c_cc[VTIME] = 10; //Set read timeout to 1 second for the connection test
			options.c_cc[VMIN] = 0; //Set minimum number of bits to read to 0;
			//Set These options
			tcsetattr(fd, TCSAFLUSH, &options);

			//Test VXM connection
			char* buffer = new char[11];
			write(fd, "V", 2);
			//if read returns without anything in the buffer, VXM is unreachable
			if (read(fd, buffer, 10) == 0) {
				connected = false;
				//Set motor controller status
				pthread_rwlock_rdlock(&runMainLock);
				id.handler->setStatusValueString(id.motorStatus, "Not Connected");
				pthread_rwlock_unlock(&runMainLock);
			} else {
				connected = true;
				//return to a blocking read
				options.c_cc[VTIME] = 0;
				options.c_cc[VMIN] = 1;
				tcsetattr(fd, TCSAFLUSH, &options);
				//Set motor controller status
				pthread_rwlock_rdlock(&runMainLock);
				id.handler->setStatusValueString(id.motorStatus, "Connected");
				pthread_rwlock_unlock(&runMainLock);
				//Put VXM in On-Line mode, enable Over-Travel monitoring level 3, and put VXM into jog mode.
				write(fd, "FO3\rQ", 5);

				//Get current positions
				readCurrentPositions(X | Y | Z, xpos, ypos, zpos);
			}
		}
	}
	~MotorControl() {
		//Close connection to serial port
		close(fd);
	}

	double stopwatch() {
		if (watchRunning) {
			timeval t;
			gettimeofday(&t, NULL);
			double stopTime = t.tv_sec + ((double) t.tv_usec) / 1000000;
			watchRunning = false;
			return (stopTime - startTime);
		} else {
			timeval t;
			gettimeofday(&t, NULL);
			startTime = t.tv_sec + ((double) t.tv_usec) / 1000000;
			watchRunning = true;
			return -1;
		}
	}

	string readFromBus() {
		string sBuffer = "";
		char* buffer = new char[101];
		int rd;

		rd = read(fd, buffer, 100);
		buffer[rd] = '\0';
		sBuffer += buffer;

		tcflush(fd, TCIOFLUSH);

		return sBuffer;
	}

	int resetControllers() {
		//if VXM not connected, return with error
		if (!connected)
			return -2;

		//Execute a VXM reset
		write(fd, "FresQ", 5);

		return 0;
	}

	int reverseDirection(unsigned int axes) {
		//reverse what the VXM interperets as positive default is away from motor
		//if axis is specified in axes, that motors direction definition is reversed.
		if (!connected)
			return -2;

		unsigned int settings = 64 | 1;

		if ((axes & X) == X) {
			if (xaxis == 1)
				settings |= 2;
			else if (xaxis == 2)
				settings |= 4;
			else if (xaxis == 3)
				settings |= 16;
		} else if ((axes & Y) == Y) {
			if (yaxis == 1)
				settings |= 2;
			else if (yaxis == 2)
				settings |= 4;
			else if (yaxis == 3)
				settings |= 16;
		} else if ((axes & Z) == Z) {
			if (zaxis == 1)
				settings |= 2;
			else if (zaxis == 2)
				settings |= 4;
			else if (zaxis == 3)
				settings |= 16;
		}

		stringstream s;
		s << "F" << settings << "\rQ";

		write(fd, s.str().c_str(), s.str().length());

		return 0;
	}

	int setAcceleration(unsigned int axes, int xAccel = 1, int yAccel = 1, int zAccel = 1) {
		//Change the acceleration of the axes to those specified in arguments, default is minimum.
		stringstream s;

		if ((axes & Z) == Z)
			s << "A" << zaxis << "M" << zAccel << "\r";
		if ((axes & X) == X)
			s << "A" << xaxis << "M" << xAccel << "\r";
		if ((axes & Y) == Y)
			s << "A" << yaxis << "M" << yAccel << "\r";

		accel = s.str();
	}

	void setSpeed(unsigned int axes, int xSpeed = 6000, int ySpeed = 6000, int zSpeed = 6000) {
		//Change the speed of the specified axes to those specified in arguments, default is maximum.
		stringstream s;

		if ((axes & Z) == Z)
			s << "S" << zaxis << "M" << zSpeed << "\r";
		if ((axes & X) == X)
			s << "S" << xaxis << "M" << xSpeed << "\r";
		if ((axes & Y) == Y)
			s << "S" << yaxis << "M" << ySpeed << "\r";

		speed = s.str();
	}

	int readCurrentPositions(unsigned int axes, int& x, int& y, int& z) {
		//Update status element positions with the current positions as saved by VXM

		if (!connected)
			return -2;

		//Connect to VXM and ensure a clear program.
		write(fd, "FC", 2);
		string sBuffer = "";
		char* buffer = new char[101];
		int rd;

		int xTemp = 0;
		int yTemp = 0;
		int zTemp = 0;
		//Check for axes
		if ((axes & X) == X) {
			string letter = "";
			if (xaxis == 1)
				letter = "X";
			else if (xaxis == 2)
				letter = "Y";
			else if (xaxis == 3)
				letter = "Z";
			write(fd, letter.c_str(), 1);
			//VXM responds with the position terminated by a carriage return, keep
			//reading until \r is seen.
			while (true) {
				rd = read(fd, buffer, 100);
				buffer[rd] = '\0';
				sBuffer += buffer;
				int pos;
				if ((pos = sBuffer.find("\r")) != string::npos) {
					sBuffer.replace(pos, 1, "\0");
					break;
				}
			}
			x = atoi(sBuffer.c_str());
			sBuffer = "";
		}

		if ((axes & Y) == Y) {
			string letter = "";
			if (yaxis == 1)
				letter = "X";
			else if (yaxis == 2)
				letter = "Y";
			else if (yaxis == 3)
				letter = "Z";
			write(fd, letter.c_str(), 1);
			while (true) {
				rd = read(fd, buffer, 100);
				buffer[rd] = '\0';
				sBuffer += buffer;
				int pos;
				if ((pos = sBuffer.find("\r")) != string::npos) {
					sBuffer.replace(pos, 1, "\0");
					break;
				}
			}
			y = atoi(sBuffer.c_str());
			sBuffer = "";
		}

		if ((axes & Z) == Z) {
			string letter = "";
			if (zaxis == 1)
				letter = "X";
			else if (zaxis == 2)
				letter = "Y";
			else if (zaxis == 3)
				letter = "Z";
			write(fd, letter.c_str(), 1);
			while (true) {
				rd = read(fd, buffer, 100);
				buffer[rd] = '\0';
				sBuffer += buffer;
				int pos;
				if ((pos = sBuffer.find("\r")) != string::npos) {
					sBuffer.replace(pos, 1, "\0");
					break;
				}
			}
			z = atoi(sBuffer.c_str());
			sBuffer = "";
		}

		//Clear commands and put VXM into jog mode.
		write(fd, "CQ", 2);

		delete[] buffer;

		tcflush(fd, TCIOFLUSH);

		return 0;
	}

	int setZeroPositions(unsigned int axes) {
		//Method tells VXM to set current position as the specified axes' Absolute zero position

		if (!connected)
			return -2;

		stringstream s;
		char* buffer = new char[101];

		s << "FC";

		if (xaxis == 3) {
			if ((axes & X) == X) {
				s << "IA" << xaxis << "M-0\r";
			}
			if ((axes & Y) == Y) {
				s << "IA" << yaxis << "M-0\r";
			}
			if ((axes & Z) == Z) {
				s << "IA" << zaxis << "M-0\r";
			}
		} else if (yaxis == 3) {
			if ((axes & Y) == Y) {
				s << "IA" << yaxis << "M-0\r";
			}
			if ((axes & Z) == Z) {
				s << "IA" << zaxis << "M-0\r";
			}
			if ((axes & X) == X) {
				s << "IA" << xaxis << "M-0\r";
			}
		} else if (zaxis == 3) {
			if ((axes & Z) == Z) {
				s << "IA" << zaxis << "M-0\r";
			}
			if ((axes & Y) == Y) {
				s << "IA" << yaxis << "M-0\r";
			}
			if ((axes & X) == X) {
				s << "IA" << xaxis << "M-0\r";
			}
		}

		s << "R";

		write(fd, s.str().c_str(), s.str().length());
		readFromBus();

		delete[] buffer;
		write(fd, "CQ", 2);

		xpos, ypos, zpos = 0;

		return 0;
	}

	int advanceToExtrema(unsigned int axes, int xDirection = 0, int yDirection = 0, int zDirection = 0) {
		//Move specified axes until the Over-Travel switches are triggered.  if the number given is negative, movement
		//is negative, otherwise it is positive.  Default is toward motor and away from motor respectively.
		/////////WARNING//////////
		//Do not call this method without lowering the default speed.

		if (!connected)
			return -2;

		cd->wait(axes);

		stringstream s;
		//O0 is called to turn off Over-Travel monitoring
		//and specified axes are moved in the specified direction
		s << "FCO0\r" << accel << speed << "(";

		if (xaxis == 3) {
			if ((axes & X) == X) {
				s << "I" << xaxis << "M";
				if (xDirection < 0)
					s << "-";
				s << "0\r";
			}
			if ((axes & Y) == Y) {
				s << "I" << yaxis << "M";
				if (yDirection < 0)
					s << "-";
				s << "0\r";
			}
			if ((axes & Z) == Z) {
				s << "I" << zaxis << "M";
				if (zDirection < 0)
					s << "-";
				s << "0\r";
			}
		} else if (yaxis == 3) {
			if ((axes & Y) == Y) {
				s << "I" << yaxis << "M";
				if (yDirection < 0)
					s << "-";
				s << "0\r";
			}
			if ((axes & Z) == Z) {
				s << "I" << zaxis << "M";
				if (zDirection < 0)
					s << "-";
				s << "0\r";
			}
			if ((axes & X) == X) {
				s << "I" << xaxis << "M";
				if (xDirection < 0)
					s << "-";
				s << "0\r";
			}
		} else if (zaxis == 3) {
			if ((axes & Z) == Z) {
				s << "I" << zaxis << "M";
				if (zDirection < 0)
					s << "-";
				s << "0\r";
			}
			if ((axes & Y) == Y) {
				s << "I" << yaxis << "M";
				if (yDirection < 0)
					s << "-";
				s << "0\r";
			}
			if ((axes & X) == X) {
				s << "I" << xaxis << "M";
				if (xDirection < 0)
					s << "-";
				s << "0\r";
			}
		}

		s << ")R";

		if (stopwatch() != -1)
			stopwatch();
		write(fd, s.str().c_str(), s.str().length());
		readFromBus();
		double time = stopwatch();

		//O3 is reinstated before entering jog mode
		write(fd, "CO3\rQ", 5);

		int x, y, z;
		readCurrentPositions(X | Y | Z, x, y, z);
		cd->registerTime(axes, time);
		xpos = x, ypos = y, zpos = z;

		return 0;
	}

	int advanceToRelativePosition(unsigned int axes, int xSteps = 0, int ySteps = 0, int zSteps = 0) {
		//Method to move to a position qSteps from the current position

		if (!connected)
			return -2;

		stringstream s;
		s << "FC" << accel << speed << "(";

		if (xaxis == 3) {
			if ((axes & X) == X) {
				if (xSteps != 0) {
					cd->wait(X);
					s << "I" << xaxis << "M" << xSteps << "\r";
				} else
					axes &= ~X;
			}
			if ((axes & Y) == Y) {
				if (ySteps != 0) {
					cd->wait(Y);
					s << "I" << yaxis << "M" << ySteps << "\r";
				} else
					axes &= ~Y;
			}
			if ((axes & Z) == Z) {
				if (zSteps != 0) {
					cd->wait(Z);
					s << "I" << zaxis << "M" << zSteps << "\r";
				} else
					axes &= ~Z;
			}
		} else if (yaxis == 3) {
			if ((axes & Y) == Y) {
				if (ySteps != 0) {
					cd->wait(Y);
					s << "I" << yaxis << "M" << ySteps << "\r";
				} else
					axes &= ~Y;
			}
			if ((axes & Z) == Z) {
				if (zSteps != 0) {
					cd->wait(Z);
					s << "I" << zaxis << "M" << zSteps << "\r";
				} else
					axes &= ~Z;
			}
			if ((axes & X) == X) {
				if (xSteps != 0) {
					cd->wait(X);
					s << "I" << xaxis << "M" << xSteps << "\r";
				} else
					axes &= ~X;
			}
		} else if (zaxis == 3) {
			if ((axes & Z) == Z) {
				if (zSteps != 0) {
					cd->wait(Z);
					s << "I" << zaxis << "M" << zSteps << "\r";
				} else
					axes &= ~Z;
			}
			if ((axes & Y) == Y) {
				if (ySteps != 0) {
					cd->wait(Y);
					s << "I" << yaxis << "M" << ySteps << "\r";
				} else
					axes &= ~Y;
			}
			if ((axes & X) == X) {
				if (xSteps != 0) {
					cd->wait(X);
					s << "I" << xaxis << "M" << xSteps << "\r";
				} else
					axes &= ~X;
			}
		}

		s << ")R";

		if (stopwatch() != -1)
			stopwatch();
		write(fd, s.str().c_str(), s.str().length());
		string errorCheck = readFromBus();
		double time = stopwatch();
		if (errorCheck.find("O") != string::npos) {
			write(fd, "CQ", 2);

			int x, y, z;
			readCurrentPositions(X | Y | Z, x, y, z);

			cd->registerTime(axes, time);
			xpos = x, ypos = y, zpos = z;

			return -1;
		}

		write(fd, "CQ", 2);

		cd->registerTime(axes, time);
		xpos += xSteps, ypos += ySteps, zpos += zSteps;

		return 0;
	}

	int returnToZeroPositions(unsigned int axes) {
		//This method returns the specified motors to the Absolute zero positions saved
		//by the VXM.

		int code = advanceToRelativePosition(axes, -xpos, -ypos, -zpos);

		return code;
	}

	int advanceToAbsolutePosition(unsigned int axes, int newxpos = 0, int newypos = 0, int newzpos = 0) {
		//Method to advance to an Absolute position relative to VXM stored zero

		int code = advanceToRelativePosition(axes, newxpos - xpos, newypos - ypos, newzpos - zpos);
		return code;
	}

	void mario() {
		//Method to play the Super Mario Bros. theme song on the 2 and 3 motors.

		if (connected) {
			pthread_rwlock_rdlock(&runMainLock);
			id.handler->setStatusValueString(id.motorStatus, "Tuning");
			pthread_rwlock_unlock(&runMainLock);
			//Set acceleration high to resonate correct note immediately.
			string songAccel = "A1M100\rA2M100\rA3M100\r";

			char* buffer = new char[101];
			int rd;
			string errorCheck = "";

			//Set zero positions so they can be returned to when the song is finished.
			setZeroPositions(X | Y | Z);

			//Increase baud rate to 38000 (max) to decrease latency between note strings
			write(fd, "FsetB38\r", 8);
			cfsetspeed(&options, B38400);
			cfmakeraw(&options);
			options.c_cflag &= ~CSTOPB;
			tcsetattr(fd, TCSAFLUSH, &options);

			//Define the song's note strings.  Each clears the buffer and sets the acceleration before defining
			//its note sequence.  Px represents a rest of x-tenths of a second.
			string song1 = "C" + songAccel + "(" + note(660, EIGHTH, 3) + note(370, EIGHTH, 2) + ")" + "("
					+ note(660, EIGHTH, 3) + note(350, EIGHTH, 2) + ")" + "P1\r" + "(" + note(660, EIGHTH,
					3) + note(350, EIGHTH, 2) + ")" + "P1\r" + "(" + note(523, EIGHTH, 3) + note(370,
					EIGHTH, 2) + ")" + "(" + note(660, QUARTER, 3) + note(370, QUARTER, 2) + ")" + "("
					+ note(740, QUARTER, 3) + note(392, QUARTER, 2) + ")" + "P3\r" + note(392, QUARTER, 2)
					+ "P3\r" + "(" + note(523, DOTQUAR, 3) + note(330, DOTQUAR, 2) + ")" + "(" + note(392,
					EIGHTH, 3) + note(330, EIGHTH, 2) + ")" + "P2\rR";

			string song2 = "C" + songAccel + "(" + note(330, DOTQUAR, 3) + note(262, DOTQUAR, 2) + ")" + "("
					+ note(440, QUARTER, 3) + note(262, QUARTER, 2) + ")" + "(" + note(494, QUARTER, 3)
					+ note(294, QUARTER, 2) + ")" + "(" + note(466, EIGHTH, 3) + note(277, EIGHTH, 2) + ")"
					+ "(" + note(440, QUARTER, 3) + note(262, QUARTER, 2) + ")" + "(" + note(392, QUARTER,
					3) + note(262, QUARTER, 2) + ")" + "(" + note(660, QUARTER, 3) + note(392, QUARTER, 2)
					+ ")" + "(" + note(740, QUARTER, 3) + note(494, QUARTER, 2) + ")" + "(" + note(880,
					QUARTER, 3) + note(523, QUARTER, 2) + ")" + "(" + note(699, EIGHTH, 3) + note(440,
					EIGHTH, 2) + ")" + "(" + note(740, EIGHTH, 3) + note(494, EIGHTH, 2) + ")" + "("
					+ note(660, QUARTER, 3) + note(440, QUARTER, 2) + ")" + "(" + note(523, EIGHTH, 3)
					+ note(330, EIGHTH, 2) + ")" + "(" + note(587, EIGHTH, 3) + note(350, EIGHTH, 2) + ")"
					+ "(" + note(494, DOTQUAR, 3) + note(294, DOTQUAR, 2) + ")P2\rR";

			string song3 = "C" + songAccel + "(" + note(783, EIGHTH, 3) + note(660, EIGHTH, 2) + ")" + "("
					+ note(740, EIGHTH, 3) + note(622, EIGHTH, 2) + ")" + "(" + note(698, EIGHTH, 3)
					+ note(587, EIGHTH, 2) + ")" + "(" + note(622, QUARTER, 3) + note(494, QUARTER, 2)
					+ ")" + "(" + note(660, EIGHTH, 3) + note(523, EIGHTH, 2) + ")" + "P1\r" + "(" + note(
					415, EIGHTH, 3) + note(330, EIGHTH, 2) + ")" + "(" + note(440, EIGHTH, 3) + note(350,
					EIGHTH, 2) + ")" + "(" + note(523, EIGHTH, 3) + note(440, EIGHTH, 2) + ")" + "P1\r" + "("
					+ note(440, EIGHTH, 3) + note(262, EIGHTH, 2) + ")" + "(" + note(523, EIGHTH, 3)
					+ note(330, EIGHTH, 2) + ")" + "(" + note(587, EIGHTH, 3) + note(350, EIGHTH, 2)
					+ ")P2\rR";

			string song4 = "C" + songAccel + "(" + note(783, EIGHTH, 3) + note(660, EIGHTH, 2) + ")" + "("
					+ note(740, EIGHTH, 3) + note(622, EIGHTH, 2) + ")" + "(" + note(698, EIGHTH, 3)
					+ note(587, EIGHTH, 2) + ")" + "(" + note(622, QUARTER, 3) + note(494, QUARTER, 2)
					+ ")" + "(" + note(660, EIGHTH, 3) + note(523, EIGHTH, 2) + ")" + "P1\r" + "(" + note(
					1047, QUARTER, 3) + note(698, QUARTER, 2) + ")" + "(" + note(1047, QUARTER, 3) + note(
					698, QUARTER, 2) + ")" + "(" + note(1047, HALF, 3) + note(698, HALF, 2) + ")P2\rR";

			string song5 = "C" + songAccel + "(" + note(622, QUARTER, 3) + note(415, QUARTER, 2) + ")" + "P1\r"
					+ "(" + note(587, QUARTER, 3) + note(349, QUARTER, 2) + ")" + "(" + note(523, HALF, 3)
					+ note(330, HALF, 2) + ")R";

			pthread_rwlock_rdlock(&runMainLock);
			id.handler->setStatusValueString(id.motorStatus, "Singing");
			pthread_rwlock_unlock(&runMainLock);

			//Send the strings and check for Over-Travel.
			write(fd, song1.c_str(), song1.length());
			rd = read(fd, buffer, 100);
			buffer[rd] = '\0';
			errorCheck = buffer;
			if (errorCheck.find("O") != string::npos) {
				write(fd, "setB9\r", 6);
				cfsetispeed(&options, B9600);
				cfsetospeed(&options, B9600);
				options.c_cflag |= (CLOCAL | CREAD);
				tcsetattr(fd, TCSAFLUSH, &options);

				delete[] buffer;
				write(fd, "CQ", 2);
			}

			write(fd, song2.c_str(), song2.length());
			rd = read(fd, buffer, 100);
			buffer[rd] = '\0';
			errorCheck = buffer;
			if (errorCheck.find("O") != string::npos) {
				write(fd, "setB9\r", 6);
				cfsetispeed(&options, B9600);
				cfsetospeed(&options, B9600);
				options.c_cflag |= (CLOCAL | CREAD);
				tcsetattr(fd, TCSAFLUSH, &options);

				delete[] buffer;
				write(fd, "CQ", 2);
			}

			write(fd, song3.c_str(), song3.length());
			rd = read(fd, buffer, 100);
			buffer[rd] = '\0';
			errorCheck = buffer;
			if (errorCheck.find("O") != string::npos) {
				write(fd, "setB9\r", 6);
				cfsetispeed(&options, B9600);
				cfsetospeed(&options, B9600);
				options.c_cflag |= (CLOCAL | CREAD);
				tcsetattr(fd, TCSAFLUSH, &options);

				delete[] buffer;
				write(fd, "CQ", 2);
			}

			write(fd, song4.c_str(), song4.length());
			rd = read(fd, buffer, 100);
			buffer[rd] = '\0';
			errorCheck = buffer;
			if (errorCheck.find("O") != string::npos) {
				write(fd, "setB9\r", 6);
				cfsetispeed(&options, B9600);
				cfsetospeed(&options, B9600);
				options.c_cflag |= (CLOCAL | CREAD);
				tcsetattr(fd, TCSAFLUSH, &options);

				delete[] buffer;
				write(fd, "CQ", 2);
			}

			write(fd, song3.c_str(), song3.length());
			rd = read(fd, buffer, 100);
			buffer[rd] = '\0';
			errorCheck = buffer;
			if (errorCheck.find("O") != string::npos) {
				write(fd, "setB9\r", 6);
				cfsetispeed(&options, B9600);
				cfsetospeed(&options, B9600);
				options.c_cflag |= (CLOCAL | CREAD);
				tcsetattr(fd, TCSAFLUSH, &options);

				delete[] buffer;
				write(fd, "CQ", 2);
			}

			write(fd, song5.c_str(), song5.length());
			rd = read(fd, buffer, 100);
			buffer[rd] = '\0';
			errorCheck = buffer;
			if (errorCheck.find("O") != string::npos) {
				write(fd, "setB9\r", 6);
				cfsetispeed(&options, B9600);
				cfsetospeed(&options, B9600);
				options.c_cflag |= (CLOCAL | CREAD);
				tcsetattr(fd, TCSAFLUSH, &options);

				delete[] buffer;
				write(fd, "CQ", 2);
			}

			//Return baud rate to default.
			write(fd, "setB9\r", 6);
			cfsetispeed(&options, B9600);
			cfsetospeed(&options, B9600);
			options.c_cflag |= (CLOCAL | CREAD);
			tcsetattr(fd, TCSAFLUSH, &options);

			sleep(1);

			//Return to original positions and clean up
			pthread_rwlock_rdlock(&runMainLock);
			id.handler->setStatusValueString(id.motorStatus, "going Home");
			pthread_rwlock_unlock(&runMainLock);
			string goHome = "C" + accel + "(S3M2000\rIA3M0\rS2M2000\rIA2M0\r)R";

			write(fd, goHome.c_str(), goHome.length());
			read(fd, buffer, 100);

			write(fd, "CQ", 2);

			pthread_rwlock_rdlock(&runMainLock);
			id.handler->setStatusValueString(id.motorStatus, "Ready");
			pthread_rwlock_unlock(&runMainLock);

			delete[] buffer;
		}
	}

private:
	Cooldown* cd;
	struct termios options;
	int fd;
	string accel;
	string speed;
	bool connected;

	int xaxis;
	int yaxis;
	int zaxis;

	int xpos;
	int ypos;
	int zpos;

	bool watchRunning;
	double startTime;

	int twoSongPos;
	int threeSongPos;
	enum noteLength {
		EIGHTH, QUARTER, DOTQUAR, HALF
	};
	string note(int frequency, int length, int motor) {
		//Method generates a tone of length dependent on the arguments.

		int duration = 0;
		int eighth = frequency * 0.14;
		stringstream s;
		s << "S" << motor << "M" << frequency << "\rI" << motor << "M";
		switch (length) {
		case HALF: {
			duration += eighth;
		}
		case DOTQUAR: {
			duration += eighth;
		}
		case QUARTER: {
			duration += eighth;
		}
		case EIGHTH: {
			duration += eighth;
		}
		}

		if (motor == 2 && twoSongPos > 0) {
			s << "-";
			twoSongPos -= duration;
		} else if (motor == 2) {
			twoSongPos += duration;
		} else if (threeSongPos > 0) {
			s << "-";
			threeSongPos -= duration;
		} else {
			threeSongPos += duration;
		}

		s << duration << "\r";

		return s.str();
	}
};

struct runInfo {
	int i;
	int j;
	int k;
	long t;
	int step;
	string fileName;
};

void* measureCenteredCube(void* voidInfo) {
	setWorker(true);
	id.handler->setStatusValueBool(id.workerRunStatus, true);
	runInfo* info = (runInfo*) voidInfo;

	MotorControl* mc = new MotorControl();
	Voltron* vt = new Voltron("192.168.0.10", "192.168.0.11", "192.168.0.12");
	if (!vt->connected() || mc->setZeroPositions(X | Y | Z) < 0) {
		delete vt;
		delete mc;
		delete info;
		setWorker(false);
		id.handler->setStatusValueBool(id.workerRunStatus, false);
		pthread_exit(NULL);
		return NULL;
	}

	TFile* file = new TFile(info->fileName.c_str(), "update");
	int i;
	for (i = 0; true; i++) {
		std::stringstream runTempSS;
		runTempSS << "run" << i;

		TTree* test = (TTree*) file->Get(runTempSS.str().c_str());
		if (test == NULL)
			break;
	}
	std::stringstream runSS;
	runSS << "run" << i;
	id.handler->setStatusValueString(id.workerRunNameStatus, runSS.str());
	TTree* tree = new TTree(runSS.str().c_str(), runSS.str().c_str());
	tree->SetAutoSave(1000000);
	double pos[3];
	TBranch* posBranch = tree->Branch("position", &pos, "pos[3]/D");
	double field[3];
	TBranch* fieldBranch = tree->Branch("field", &field, "field[3]/D");
	double stepcm = ((double) info->step) / 400.0 * 0.254;
	TBranch* stepBranch = tree->Branch("step", &stepcm, "step/D");
	stepBranch->Fill();

	int iLow = -info->i / 2;
	int jLow = -info->j / 2;
	int kLow = -info->k / 2;
	long startT = time(NULL);
	for (int k = 0; k <= info->k; k += info->step) {
		for (int i = 0; i <= info->i; i += info->step) {
			for (int j = 0; j <= info->j; j += info->step) {
				int relI;
				int relJ;
				if ((i + k) % (2 * info->step) == 0)
					relJ = j;
				else
					relJ = info->j - j;
				if (k % (2 * info->step) == 0)
					relI = i;
				else
					relI = info->i - i;

				int status, x, y, z;
				status = mc->advanceToAbsolutePosition(X | Y | Z, iLow + relI, jLow + relJ, kLow + k);
				mc->readCurrentPositions(X | Y | Z, x, y, z);
				id.handler->setStatusValueInt(id.xStatMC, x);
				id.handler->setStatusValueInt(id.yStatMC, y);
				id.handler->setStatusValueInt(id.zStatMC, z);

				if (status < 0 || !runWorker()) {
					i = info->i + 1;
					j = info->j + 1;
					k = info->k + 1;
				}

				id.handler->setStatusValueString(id.mm10vX, vt->read10V(X));
				id.handler->setStatusValueString(id.mm10vY, vt->read10V(Y));
				id.handler->setStatusValueString(id.mm10vZ, vt->read10V(Z));
				id.handler->setStatusValueInt(id.workerRunTimeStatus, time(NULL) - startT);

				field[0] = atof(id.handler->getStatusValueString(id.mm10vX).c_str()) / 10.0;
				field[1] = atof(id.handler->getStatusValueString(id.mm10vY).c_str()) / 10.0;
				field[2] = atof(id.handler->getStatusValueString(id.mm10vZ).c_str()) / 10.0;
				fieldBranch->Fill();

				pos[0] = ((double) (iLow + relI)) / 400.0 * 0.254;
				pos[1] = ((double) (jLow + relJ)) / 400.0 * 0.254;
				pos[2] = ((double) (kLow + k)) / 400.0 * 0.254;
				posBranch->Fill();
			}
		}
	}

	tree->AutoSave("SaveSelf");
	file->Close();
	mc->returnToZeroPositions(X | Y | Z);

	delete vt;
	delete mc;
	delete info;
	setWorker(false);
	id.handler->setStatusValueBool(id.workerRunStatus, false);
	pthread_exit(NULL);
	return NULL;
}

void* measureCenteredTube(void* voidInfo) {
	setWorker(true);
	id.handler->setStatusValueBool(id.workerRunStatus, true);
	runInfo* info = (runInfo*) voidInfo;

	MotorControl* mc = new MotorControl();
	Voltron* vt = new Voltron("192.168.0.10", "192.168.0.11", "192.168.0.12");
	if (!vt->connected() || mc->setZeroPositions(X | Y | Z) < 0) {
		delete vt;
		delete mc;
		delete info;
		setWorker(false);
		id.handler->setStatusValueBool(id.workerRunStatus, false);
		pthread_exit(NULL);
		return NULL;
	}

	TFile* file = new TFile(info->fileName.c_str(), "update");
	int i;
	for (i = 0; true; i++) {
		std::stringstream runTempSS;
		runTempSS << "run" << i;

		TTree* test = (TTree*) file->Get(runTempSS.str().c_str());
		if (test == NULL) {
			break;
		}
	}
	std::stringstream runSS;
	runSS << "run" << i;
	id.handler->setStatusValueString(id.workerRunNameStatus, runSS.str());
	TTree* tree = new TTree(runSS.str().c_str(), runSS.str().c_str());
	tree->SetAutoSave(1000000);
	double pos[3];
	TBranch* posBranch = tree->Branch("position", &pos, "pos[3]/D");
	double field[3];
	TBranch* fieldBranch = tree->Branch("field", &field, "field[3]/D");
	double stepcm = ((double) info->step) / 400.0 * 0.254;
	TBranch* stepBranch = tree->Branch("step", &stepcm, "step/D");
	stepBranch->Fill();

	int radius = info->i;
	info->i = 2 * info->i;
	info->j = 2 * info->i;
	int iLow = -info->i / 2;
	int jLow = -info->j / 2;
	int kLow = -info->k / 2;
	long startT = time(NULL);
	for (int k = 0; k <= info->k; k += info->step) {
		for (int i = 0; i <= info->i; i += info->step) {
			for (int j = 0; j <= info->j; j += info->step) {
				int relI;
				int relJ;
				if ((i + k) % (2 * info->step) == 0) {
					relJ = j;
				} else {
					relJ = info->j - j;
				}
				if (k % (2 * info->step) == 0) {
					relI = i;
				} else {
					relI = info->i - i;
				}

				int iPos = iLow + relI;
				int jPos = jLow + relJ;
				int kPos = kLow + k;

				if (sqrt(iPos * iPos + jPos * jPos) > radius) {
					continue;
				}

				int status, x, y, z;
				status = mc->advanceToAbsolutePosition(X | Y | Z, iPos, jPos, kPos);
				mc->readCurrentPositions(X | Y | Z, x, y, z);
				id.handler->setStatusValueInt(id.xStatMC, x);
				id.handler->setStatusValueInt(id.yStatMC, y);
				id.handler->setStatusValueInt(id.zStatMC, z);

				if (status < 0 || !runWorker()) {
					i = info->i + 1;
					j = info->j + 1;
					k = info->k + 1;
				}

				id.handler->setStatusValueString(id.mm10vX, vt->read10V(X));
				id.handler->setStatusValueString(id.mm10vY, vt->read10V(Y));
				id.handler->setStatusValueString(id.mm10vZ, vt->read10V(Z));
				id.handler->setStatusValueInt(id.workerRunTimeStatus, time(NULL) - startT);

				field[0] = atof(id.handler->getStatusValueString(id.mm10vX).c_str()) / 10.0;
				field[1] = atof(id.handler->getStatusValueString(id.mm10vY).c_str()) / 10.0;
				field[2] = atof(id.handler->getStatusValueString(id.mm10vZ).c_str()) / 10.0;
				fieldBranch->Fill();

				pos[0] = ((double) iPos) / 400.0 * 0.254;
				pos[1] = ((double) jPos) / 400.0 * 0.254;
				pos[2] = ((double) kPos) / 400.0 * 0.254;
				posBranch->Fill();
			}
		}
	}

	tree->AutoSave("SaveSelf");
	file->Close();
	mc->returnToZeroPositions(X | Y | Z);

	delete vt;
	delete mc;
	delete info;
	setWorker(false);
	id.handler->setStatusValueBool(id.workerRunStatus, false);
	pthread_exit(NULL);
	return NULL;
}

void* measureOverTime(void* voidInfo) {
	setWorker(true);
	id.handler->setStatusValueBool(id.workerRunStatus, true);
	runInfo* info = (runInfo*) voidInfo;

	Voltron* vt = new Voltron("192.168.0.10", "192.168.0.11", "192.168.0.12");
	if (!vt->connected()) {
		delete vt;
		delete info;
		setWorker(false);
		id.handler->setStatusValueBool(id.workerRunStatus, false);
		pthread_exit(NULL);
		return NULL;
	}

	TFile* file = new TFile(info->fileName.c_str(), "update");
	int i;
	for (i = 0; true; i++) {
		std::stringstream runTempSS;
		runTempSS << "run" << i;

		TTree* test = (TTree*) file->Get(runTempSS.str().c_str());
		if (test == NULL)
			break;
	}
	std::stringstream runSS;
	runSS << "run" << i;
	id.handler->setStatusValueString(id.workerRunNameStatus, runSS.str());
	TTree* tree = new TTree(runSS.str().c_str(), runSS.str().c_str());
	tree->SetAutoSave(1000000);
	double field[3];
	TBranch* fieldBranch = tree->Branch("field", &field, "field[3]/D");
	long timeHolder;
	TBranch* timeBranch = tree->Branch("time", &timeHolder, "time/L");

	long startT = time(NULL);
	while (time(NULL) < startT + info->t) {
		id.handler->setStatusValueString(id.mm10vX, vt->read10V(X));
		id.handler->setStatusValueString(id.mm10vY, vt->read10V(Y));
		id.handler->setStatusValueString(id.mm10vZ, vt->read10V(Z));
		id.handler->setStatusValueInt(id.workerRunTimeStatus, time(NULL) - startT);

		field[0] = atof(id.handler->getStatusValueString(id.mm10vX).c_str()) / 10.0;
		field[1] = atof(id.handler->getStatusValueString(id.mm10vY).c_str()) / 10.0;
		field[2] = atof(id.handler->getStatusValueString(id.mm10vZ).c_str()) / 10.0;
		fieldBranch->Fill();

		timeHolder = time(NULL) - startT;
		timeBranch->Fill();
	}

	tree->AutoSave("SaveSelf");
	file->Close();

	delete vt;
	delete info;
	setWorker(false);
	id.handler->setStatusValueBool(id.workerRunStatus, false);
	pthread_exit(NULL);
	return NULL;
}

bool readPositions() {
	if (id.handler->getStatusValueBool(id.workerRunStatus)) {
		return false;
	}
	bool x = id.handler->getControlValueBool(id.readCore, id.xReadCore);
	bool y = id.handler->getControlValueBool(id.readCore, id.yReadCore);
	bool z = id.handler->getControlValueBool(id.readCore, id.zReadCore);

	int xpos, ypos, zpos;
	MotorControl* mc = new MotorControl();
	if (x && y && z) {
		mc->readCurrentPositions(X | Y | Z, xpos, ypos, zpos);
		id.handler->setStatusValueInt(id.xStatMC, xpos);
		id.handler->setStatusValueInt(id.yStatMC, ypos);
		id.handler->setStatusValueInt(id.zStatMC, zpos);
	} else if (x && y) {
		mc->readCurrentPositions(X | Y, xpos, ypos, zpos);
		id.handler->setStatusValueInt(id.xStatMC, xpos);
		id.handler->setStatusValueInt(id.yStatMC, ypos);
	} else if (y && z) {
		mc->readCurrentPositions(Y | Z, xpos, ypos, zpos);
		id.handler->setStatusValueInt(id.yStatMC, ypos);
		id.handler->setStatusValueInt(id.zStatMC, zpos);
	} else if (z && x) {
		mc->readCurrentPositions(Z | X, xpos, ypos, zpos);
		id.handler->setStatusValueInt(id.xStatMC, xpos);
		id.handler->setStatusValueInt(id.zStatMC, zpos);
	} else if (x) {
		mc->readCurrentPositions(X, xpos, ypos, zpos);
		id.handler->setStatusValueInt(id.xStatMC, xpos);
	} else if (y) {
		mc->readCurrentPositions(Y, xpos, ypos, zpos);
		id.handler->setStatusValueInt(id.yStatMC, ypos);
	} else if (z) {
		mc->readCurrentPositions(Z, xpos, ypos, zpos);
		id.handler->setStatusValueInt(id.zStatMC, zpos);
	}

	delete mc;

	return true;
}

bool returnToZeroPositions() {
	if (id.handler->getStatusValueBool(id.workerRunStatus)) {
		return false;
	}
	bool x = id.handler->getControlValueBool(id.zeroCore, id.xZeroCore);
	bool y = id.handler->getControlValueBool(id.zeroCore, id.yZeroCore);
	bool z = id.handler->getControlValueBool(id.zeroCore, id.zZeroCore);

	MotorControl* mc = new MotorControl();
	if (x && y && z)
		mc->returnToZeroPositions(X | Y | Z);
	else if (x && y)
		mc->returnToZeroPositions(X | Y);
	else if (y && z)
		mc->returnToZeroPositions(Y | Z);
	else if (z && x)
		mc->returnToZeroPositions(Z | X);
	else if (x)
		mc->returnToZeroPositions(X);
	else if (y)
		mc->returnToZeroPositions(Y);
	else if (z)
		mc->returnToZeroPositions(Z);

	delete mc;

	readPositions();
	return true;
}

bool advance() {
	int x = id.handler->getControlValueInt(id.advance, id.xad);
	int y = id.handler->getControlValueInt(id.advance, id.yad);
	int z = id.handler->getControlValueInt(id.advance, id.zad);

	MotorControl* mc = new MotorControl();
	int error = mc->advanceToRelativePosition(X | Y | Z, x, y, z);
	if (error < 0) {
		return false;
	}

	return true;
}

void start(ModuleHandler* handler) {
	pthread_rwlock_init(&runMainLock, NULL);
	pthread_rwlock_init(&runWorkerLock, NULL);
	runMainBool = true;
	runWorkerBool = false;
	joinWorker = false;

	handler->setModuleDescription("Magnetic Field Mapping");

	char* DATA_PATH_ENV = getenv("MFM_DATA_PATH");
	if (DATA_PATH_ENV)
		DATA_PATH = DATA_PATH_ENV;
	else {
		DATA_PATH = getenv("HOME");
		DATA_PATH += "/MFMData";
	}

	struct stat st;
	string dir = DATA_PATH;
	DATA_PATH += "/";

	if(stat(dir.c_str(),&st) != 0)
		if(mkdir(dir.c_str(),0775) != 0)
			return;
	else if (!S_ISDIR(st.st_mode))
		return;

	id.handler = handler;
	id.motorStatus = handler->addStatusElement(STRINGSTATUS, "Status of motors");
	id.xStatMC = handler->addStatusElement(INTSTATUS, "X position (steps)");
	id.yStatMC = handler->addStatusElement(INTSTATUS, "Y position (steps)");
	id.zStatMC = handler->addStatusElement(INTSTATUS, "Z position (steps)");
	id.readCore = handler->addControlElement("Read the current positions of motors", "");
	id.xReadCore = handler->addControl(id.readCore, CHECKBOXCONTROL, "x");
	handler->setControlValueBool(id.readCore, id.xReadCore, true);
	id.yReadCore = handler->addControl(id.readCore, CHECKBOXCONTROL, "y");
	handler->setControlValueBool(id.readCore, id.yReadCore, true);
	id.zReadCore = handler->addControl(id.readCore, CHECKBOXCONTROL, "z");
	handler->setControlValueBool(id.readCore, id.zReadCore, true);
	id.zeroCore = handler->addControlElement("Return axes to saved zero positions", "");
	id.xZeroCore = handler->addControl(id.zeroCore, CHECKBOXCONTROL, "x");
	handler->setControlValueBool(id.zeroCore, id.xZeroCore, true);
	id.yZeroCore = handler->addControl(id.zeroCore, CHECKBOXCONTROL, "y");
	handler->setControlValueBool(id.zeroCore, id.yZeroCore, true);
	id.zZeroCore = handler->addControl(id.zeroCore, CHECKBOXCONTROL, "z");
	handler->setControlValueBool(id.zeroCore, id.zZeroCore, true);
	//	 id.marioControl = handler->addControlElement("Mario Theme", "play");

	id.MCCContElem = handler->addControlElement("Measure centered cube", "Go");
	id.MCCIRangeCont = handler->addControl(id.MCCContElem, INTCONTROL, "i range");
	handler->setControlValueInt(id.MCCContElem, id.MCCIRangeCont, 16000);
	id.MCCJRangeCont = handler->addControl(id.MCCContElem, INTCONTROL, "j range");
	handler->setControlValueInt(id.MCCContElem, id.MCCJRangeCont, 16000);
	id.MCCKRangeCont = handler->addControl(id.MCCContElem, INTCONTROL, "k range");
	handler->setControlValueInt(id.MCCContElem, id.MCCKRangeCont, 36000);
	id.MCCStep = handler->addControl(id.MCCContElem, INTCONTROL, "Step size");
	handler->setControlValueInt(id.MCCContElem, id.MCCStep, 1000);
	id.MCCFile = handler->addControl(id.MCCContElem, STRINGCONTROL, "Data file name from " + DATA_PATH);
	handler->setControlValueString(id.MCCContElem, id.MCCFile, "MCCData.root");

	id.MCTContElem = handler->addControlElement("Measure centered TUBE", "Go");
	id.MCTIRangeCont = handler->addControl(id.MCTContElem, INTCONTROL, "radius");
	handler->setControlValueInt(id.MCTContElem, id.MCTIRangeCont, 8000);
	id.MCTKRangeCont = handler->addControl(id.MCTContElem, INTCONTROL, "k range");
	handler->setControlValueInt(id.MCTContElem, id.MCTKRangeCont, 36000);
	id.MCTStep = handler->addControl(id.MCTContElem, INTCONTROL, "Step size");
	handler->setControlValueInt(id.MCTContElem, id.MCTStep, 1000);
	id.MCTFile = handler->addControl(id.MCTContElem, STRINGCONTROL, "Data file name from " + DATA_PATH);
	handler->setControlValueString(id.MCTContElem, id.MCTFile, "MCTData.root");

	id.MOTContElem = handler->addControlElement("Measure Over Time", "Go");
	id.MOTTimeCont = handler->addControl(id.MOTContElem, INTCONTROL, "time to run (s)");
	handler->setControlValueInt(id.MOTContElem, id.MOTTimeCont, 3600);
	id.MOTFileCont = handler->addControl(id.MOTContElem, STRINGCONTROL, "Data file name from " + DATA_PATH);
	handler->setControlValueString(id.MOTContElem, id.MOTFileCont, "MOTData.root");

	id.workerRunElem = handler->addControlElement("Stop worker thread", "Stop");
	id.workerRunStatus = handler->addStatusElement(BOOLSTATUS, "Worker thread running");
	id.workerRunTimeStatus = handler->addStatusElement(INTSTATUS, "Time in seconds worker thread has run");
	id.workerRunNameStatus = handler->addStatusElement(STRINGSTATUS, "Run name");

	id.mm10vX = handler->addStatusElement(STRINGSTATUS, "10V Range X Voltage");
	id.mm10vY = handler->addStatusElement(STRINGSTATUS, "10V Range Y Voltage");
	id.mm10vZ = handler->addStatusElement(STRINGSTATUS, "10V Range Z Voltage");
	id.mmRead = handler->addControlElement("Update 10V Voltage Status", "Update");

	id.advance = handler->addControlElement("move to relative positions", "move");
	id.xad = handler->addControl(id.advance, INTCONTROL, "x");
	id.yad = handler->addControl(id.advance, INTCONTROL, "y");
	id.zad = handler->addControl(id.advance, INTCONTROL, "z");

	id.marioControl = handler->addControlElement("sing the Mario theme song","play");

	MotorControl* mc = new MotorControl;
	int x, y, z;
	mc->readCurrentPositions(X | Y | Z, x, y, z);
	id.handler->setStatusValueInt(id.xStatMC, x);
	id.handler->setStatusValueInt(id.yStatMC, y);
	id.handler->setStatusValueInt(id.zStatMC, z);
	delete mc;

	while (runMain()) {
		string response = "";
		string path = "";
		int pushedID = handler->waitForButtonPush();
		if (pushedID == id.mmRead) {
			Voltron* vt = new Voltron("192.168.0.10", "192.168.0.11", "192.168.0.12");
			handler->setStatusValueString(id.mm10vX, vt->read10V(X));
			handler->setStatusValueString(id.mm10vY, vt->read10V(Y));
			handler->setStatusValueString(id.mm10vZ, vt->read10V(Z));
			delete vt;
			response = "Voltages updated";
		} else if (pushedID == id.MCCContElem) {
			if (handler->getStatusValueBool(id.workerRunStatus))
				response = "Worker thread already running";
			else {
				if (joinWorker) {
					joinWorker = false;
					pthread_join(worker, NULL);
				}
				runInfo* info = new runInfo;
				info->i = handler->getControlValueInt(id.MCCContElem, id.MCCIRangeCont);
				info->j = handler->getControlValueInt(id.MCCContElem, id.MCCJRangeCont);
				info->k = handler->getControlValueInt(id.MCCContElem, id.MCCKRangeCont);
				info->step = handler->getControlValueInt(id.MCCContElem, id.MCCStep);
				info->fileName = DATA_PATH + handler->getControlValueString(id.MCCContElem, id.MCCFile);
				pthread_create(&worker, NULL, measureCenteredCube, info);
				joinWorker = true;
				response = "Worker thread created";
			}
		} else if (pushedID == id.MCTContElem) {
			if (handler->getStatusValueBool(id.workerRunStatus))
				response = "Worker thread already running";
			else {
				if (joinWorker) {
					joinWorker = false;
					pthread_join(worker, NULL);
				}
				runInfo* info = new runInfo;
				info->i = handler->getControlValueInt(id.MCTContElem, id.MCTIRangeCont);
				info->k = handler->getControlValueInt(id.MCTContElem, id.MCTKRangeCont);
				info->step = handler->getControlValueInt(id.MCTContElem, id.MCTStep);
				info->fileName = DATA_PATH + handler->getControlValueString(id.MCTContElem, id.MCTFile);
				pthread_create(&worker, NULL, measureCenteredTube, info);
				joinWorker = true;
				response = "Worker thread created";
			}
		} else if (pushedID == id.MOTContElem) {
			if (handler->getStatusValueBool(id.workerRunStatus))
				response = "Worker thread already running";
			else {
				if (joinWorker) {
					joinWorker = false;
					pthread_join(worker, NULL);
				}
				runInfo* info = new runInfo;
				info->t = handler->getControlValueInt(id.MOTContElem, id.MOTTimeCont);
				info->fileName = DATA_PATH + handler->getControlValueString(id.MOTContElem, id.MOTFileCont);
				pthread_create(&worker, NULL, measureOverTime, info);
				joinWorker = true;
				response = "Worker thread created";
			}
		} else if (pushedID == id.readCore) {
			if (readPositions())
				response = "Positions updated";
			else
				response = "Failed";
		} else if (pushedID == id.workerRunElem) {
			if (runWorker()) {
				response = "Stopping...";
				setWorker(false);
			} else
				response = "Worker thread not running, or is currently stopping";
		} else if (pushedID == id.zeroCore) {
			if (returnToZeroPositions())
				response = "Success";
			else
				response = "Failed";
		} else if (pushedID == id.advance) {
			if (advance())
				response = "Success";
			else
				response = "Failed";
		} else if (pushedID == id.marioControl) {
			MotorControl* mc = new MotorControl;
			mc->mario();
			delete mc;
			response = "Did you like it?";
		} else{
			;
		}
		handler->giveResponse(response, path);
	}

	if (joinWorker) {
		joinWorker = false;
		pthread_join(worker, NULL);
	}

	pthread_rwlock_destroy(&runMainLock);
	pthread_rwlock_destroy(&runWorkerLock);
}

void stop(ModuleHandler* handler) {
	setWorker(false);
	setMain(false);
	handler->abandonButtonWait();
}
